#include "at_fhooe_tof_fileread_TOFStreaming.h"
#include "pmdsdk2.h"
#include <string.h>

#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>



// on error, prepend absolute path to files before plugin names
#define SOURCE_PLUGIN "digicam"
#define SOURCE_PARAM ""
#define PROC_PLUGIN "digicamproc"
#define PROC_PARAM ""

char err[128];

int running = 1;

PMDHandle hnd;
int res;


//***************** Init ******************//
JNIEXPORT void JNICALL Java_at_fhooe_tof_fileread_TOFStreaming_initStream
  (JNIEnv *env, jobject obj){

	res = pmdOpen (&hnd, SOURCE_PLUGIN, SOURCE_PARAM, PROC_PLUGIN, PROC_PARAM);
	if (res != PMD_OK)
    {
      pmdGetLastError (0, err, 128);
      printf("Could not connect\n");
      //fprintf (stderr, "Could not connect: %s\n", err);
    }
	//printf("xx %08lx\n",(unsigned int)hnd);
	//printf("opened sensor\n");
	fflush(stdout);

  }



//************************* Streaming ******************************//
JNIEXPORT jfloatArray JNICALL Java_at_fhooe_tof_fileread_TOFStreaming_getStream
  (JNIEnv *env, jobject obj)
{
	res = pmdUpdate (hnd);
	//printf("xx %08lx\n",(unsigned int)hnd);
	if (res != PMD_OK) {
		pmdGetLastError (hnd, err, 128);
		printf("getStream %d: Could not transfer data\n",__LINE__,err);
		printf("%d\n",res);
		//fprintf (stderr, "Could not transfer data: %s\n", err);
		pmdClose (hnd);
	}

	PMDDataDescription dd;
	res = pmdGetSourceDataDescription (hnd, &dd);
	if (res != PMD_OK) {
		pmdGetLastError (hnd, err, 128);
		printf("getStream %d: Could not get data description\n",__LINE__,err);
		//fprintf (stderr, "Could not get data description: %s\n", err);
		pmdClose (hnd);
	} 

	if (dd.subHeaderType != PMD_IMAGE_DATA) {
		printf ("getStream %d: Source data is not an image!\n",__LINE__,err);
		pmdClose (hnd);
	}
	//printf ("getStream %d: Jfloat error!\n",__LINE__);
	jfloatArray distArray = env->NewFloatArray(dd.img.numRows * dd.img.numColumns);
	jfloat * dist = new jfloat [dd.img.numRows * dd.img.numColumns];
	//printf ("getStream %d: Jfloat error!\n",__LINE__);
	res = pmdGetDistances (hnd, dist, dd.img.numColumns * dd.img.numRows * sizeof (float));
	if (res != PMD_OK) {
		pmdGetLastError (hnd, err, 128);
		printf ("getStream %d: Could not get distances: %s\n",__LINE__,err);
		pmdClose (hnd);
	}

	//printf ("Middle distance: %f m\n", dist[(dd.img.numRows / 2) * dd.img.numColumns + dd.img.numColumns / 2]);
	env->SetFloatArrayRegion(distArray,0,dd.img.numRows * dd.img.numColumns,dist);
	delete[] dist;
  
	return distArray;
}

//********************* Close ***********************//
JNIEXPORT void JNICALL Java_at_fhooe_tof_fileread_TOFStreaming_close
  (JNIEnv *env, jobject obj){
  printf("stopping.\n");

  pmdClose (hnd);
}

//void main(){}
